#include "sample.h"

vector<float> p(5, 0.2);
vector<string> world = {"green", "red", "red", "green", "green"};
vector<string> measurements = {"red", "green"};
vector<int> motions = {1, 1};
float pHint = 0.6;
float pMiss = 0.2;
float pExact = 0.8;
float pOvershoot = 0.1;
float pUndershoot = 0.1;

vector<float> sense(vector<float> update_before, string measurement_value) {
    unsigned long size = update_before.size();
    int hit = 0;
    vector<float> result(0, size);
    for (int i = 0; i < size; i++) {
        if (measurement_value == world[i]) {
            hit = 1;
        } else {
            hit = 0;
        }
        result.push_back(update_before[i] * (hit * pHint + (1 - hit) * pMiss));
    }

    float sum = accumulate(result.begin(), result.end(), float(0));

    for (int i = 0; i < size; i++) {
        result[i] = result[i] / sum;
    }

    return result;
}

vector<float> move(vector<float> move_before, int step) {
    unsigned long size = move_before.size();
    int hit = 0;
    vector<float> result(0, size);
    float tmp = 0.0;
    for (int i = 0; i < size; i++) {
        tmp = pExact * move_before[(i - step) % size];
        tmp = tmp + pOvershoot * move_before[(i - step - 1) % size];
        tmp = tmp + pUndershoot * move_before[(i - step + 1) % size];
        result.push_back(tmp);
    }
    return result;
}

void trans() {
    unsigned long size = measurements.size();
    for (int i = 0; i < size; i++) {
        p = sense(p, measurements[i]);
        p = move(p, motions[i]);
    }
    for (float i:p) {
        cout << i << endl;
    }
}
